'''
Author: your name
Date: 2021-07-25 21:27:14
LastEditTime: 2021-07-25 21:27:14
LastEditors: Please set LastEditors
Description: In User Settings Edit
FilePath: \FFTfly\MyOpenmv\April.py
'''
#串口加Apriltag

import sensor, image, time,math
from pyb import UART

uart = UART(3,115200)


sensor.reset()                      # Reset and initialize the sensor.
sensor.set_pixformat(sensor.RGB565) # Set pixel format to RGB565 (or GRAYSCALE)
sensor.set_framesize(sensor.QQVGA)   # Set frame size to QVGA (320x240)
sensor.skip_frames(time = 2000)     # Wait for settings take effect.
clock = time.clock()                # Create a clock object to track the FPS.
#sensor.set_vflip(True)
#sensor.set_hmirror(True)

def pack_test_data(x, y):

    pack_data=bytearray([x,y,0X00])

    lens = len(pack_data)#数据包大小


    i = 0
    sum = 0

    #和校验
    while i<(lens-1):
        sum = sum + pack_data[i]
        i = i+1
    pack_data[lens-1] = sum

    return pack_data



while(True):
    clock.tick()                    # Update the FPS clock.
    img = sensor.snapshot()         # Take a picture and return the image.

    tag = img.find_apriltags() # defaults to TAG36H11 without "families".
    (X, Y) = (255, 255)
    if(len(tag) >= 1):
        (X, Y) = (tag[0].cx(), tag[0].cy())
    while(not uart.any()):
        pass
    if uart.readchar() == 0xAA:
        if (X,Y) == (255,255):
            uart.write(pack_test_data(255, 255))
        else:
            uart.write(pack_test_data(159 - Y, 239 - X))

    print(119 - X, 80 - Y)
